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Abstract 

In this paper, we investigate the applicability of conjugate gradient 
algorithms for computation of the manipulator forward dynamics. The 
redundancies in the previously proposed conjugate gradient algorithm is 
analyzed [7], A new version is developed which, by avoiding these 
redundancies, achieves a significantly greater efficiency. A preconditioned 
conjugate gradient algorithm is also presented. A diagonal matrix whose 
elements are the diagonal elements of the inertia matrix is proposed as the 
preconditioner. In order to increase the computational efficiency, an 
algorithm is developed which exploits the synergism between the computation 
of the diagonal elements of the inertia matrix and that required by the 
conjugate gradient algorithm. 

I. INTRODUCTION 

The manipulator forward dynamics problem, which concerns the determination 
of the motion resulting from the application of a set of joint forces/ 
torques, is essential for the dynamic simulation of robot manipulators. The 
motivation for devising fast algorithms for the forward dynamics solution 
stems from applications which require extensive off-line simulation as well 
as applications which require real-time dynamic simulation. In particular, 
for many anticipated space teleoperation applications, a faster-than-real- 
time simulation capability will be essential. In fact, in the presence of 
the unavoidable delay in information transfer, such a capability would allow 
a human operator to preview a number of scenarios before run-time [1]. 

The forward dynamics problem can be stated as follows: given the vector 
of the actual joint positions (Q) and velocities (Q), and the vector of 
applied joint forces/torques (x), find the vector of the joint accelerations 
(Q). Integrating Q leads to the new values for Q and Q. The process is then 
repeated for the next x. The first step in the computation of the forward 
dynamics is to derive a linear relation (for the given Q) between the vector 
of joint accelerations and the vector of joint inertia forces/torques. Given 
the dynamic equations of motion as 

A(Q)Q + C(Q, Q) + G(Q) + /(Q)F £ = x (1) 

and the bias vector (b) as 

b = C(Q,Q) + G(Q) + /(Q)F £ (2) 

the linear relation is derived as 

A(Q)Q = x - b = T (3) 

where A(Q) is an nxn symmetric, positive definite, inertia matrix and J is 
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the 6xn Jacobian matrix (t denote matrix transpose). Q, Q, Q, T , b, r€(R n , 
and F e is the 6x1 vector which is a compact representation of the external 

force (f E ) and moment (n E ) exerted on the End-Effector (EE). The bias vector 

represents the contribution due to the nonlinear terms as well as the 
external force and moment. Hence, T stands for the vector of applied inertia 
forces/torques. The bias vector can be obtained by computing the inverse 
dynamics, using the Newton-Euler (N-E) formulation [2], for the actual value 
of Q, Q, and F e while setting Q to zero. The evaluation of b and r, i.e., 

the derivation of Eq. (3), is necessarily the first step in the computation 
of forward dynamics. 

The proposed algorithms for computation of the forward dynamics differ 
in their approaches to solving Eq. (3), which directly affect their 
asymptotic computational complexity. These algorithms can be classified as 

0(n) algorithms [3]-[6], the 0( n 2 ) algorithms [71, and the 0(n 3 ) algorithms 
[71. However, any analysis of the efficiency of these algorithms should be 
based on the realistic size of the problem, i.e. the number of Degrees-Of- 

Freedom (DOF). In fact, the comparative study in [3] shows that the 0(n 3 ) 
composite rigid-body algorithm is the most efficient for n less than 12. It 

also shows that, due to the large coefficient of n 2 terms on the polynomial 
complexity, the conjugate gradient algorithm of [7] does not become more 
efficient than the composite rigid-body algorithm except for very large n, 
making the algorithm almost impractical. 

In this paper, we develop two conjugate gradient algorithms which are 
significantly more efficient than that of [71. The better efficiency of 
these algorithms is mainly achieved by a significant reduction of the 
2 

coefficient of n terms on the polynomial complexity. The first is a 
Classical Conjugate Gradient (CCG) algorithm which improves the computation 
cost of each iteration by eliminating the redundancy in the extrinsic 
equations, i.e., by a better choice of coordinate frame for projection of 
the intrinsic equations. With this reduction in the cost of each iteration, 
a further efficiency can be achieved by reducing the number of iterations 
through the use of a preconditioner. The second is a Preconditioned 
Conjugate Gradient (PCG) algorithm which uses a positive definite diagonal 
matrix, whose elements are the diagonal elements of the inertia matrix, as a 
preconditioner. An efficient algorithm for computation of the diagonal 
elements of the inertia matrix is also developed. 

However, despite these improvements, the developed algorithms are, in 

general, still less efficient than the best 0(n 3 ) algorithm. It should be 
pointed out that the efficiency of this algorithm is further increased by a 
recently developed algorithm [8]-[9] which achieves greater efficiency in 
computing the inertia matrix over the composite rigid-body algorithm in [7]. 
Despite the improvement in the efficiency of the serial algorithms, even the 
fastest serial algorithm is far from providing the required efficiency for 
real-time or faster-than-real-t ime simulation. This observation clearly 
suggests that the exploitation of parallelism in the computation is the key 
factor in achieving the desired efficiency. 


330 


I 



The analysis of the parallel efficiency of different algorithms is more 
complex than that of the serial efficiency [9]. Our Investigation indicates 
that the PCG algorithm presents excellent features for parallel computation 
[10]. In fact, the parallel version of the PCG algorithm, while requiring a 
simple architecture, may potentially become the most efficient alternative 
for parallel computation of the forward dynamics. In fact, such a potential 
has motivated us to further investigate the PCG algorithm and the impact of 
the preconditioning on its convergence. In this paper the preliminary 
results of our investigation are presented. 

This paper is organized as follows. In Section II, the CCG and PCG 
algorithms are briefly reviewed and the particular features of these 
algorithms in the context of the forward dynamics computation are discussed. 
In Section III, the CCG algorithm is developed. In Section IV, the PCG 
algorithm and the algorithm for computation of the diagonal elements of the 
inertia matrix are presented. Finally, some discussion and concluding 
remarks are made in Section V. 

II. CONJUGATE GRADIENT METHOD AND RESULTING ALGORITHMS 

The conjugate gradient method is one of the most widely used methods for 
the iterative solution of linear systems of equations such as 

Ax = b x, b € R n (4) 

where Ae R nxn is a symmetric positive-definite matrix. An attractive feature 
of the method is the guarantee of the convergence in at most n steps. Several 
developments have contributed to the wide application of the method [13]; 
they include analysis and experimentation leading to the identification of 
the most stable versions of the method, an understanding of its error 
propagation, and the fact that the solution of Eq. (4) arises in many 
applications. 

The discussion given here is mainly based on the treatment found in [12] 
where the basic algorithm is given as follows: 

x = 0 

o 

r = 0 

o 

For j = 1, 2, . . . , n 

if r =0 then set x = x and quit 
j-i j-i 

else 


13 

t , t 

= r r /r r 

o 

in 

cd 

(5) 

i 

J-l J-l j-2 j-2 

1 


P J 

= r + |3 p 

J-i J*J-i 

p l s 0 

(6) 

a 

J 

= r r /p Ap 
j-i j-i *j K j 


(7) 

X 

J 

= x + a p 

J-i J J 


(8) 

r 

J 

= r - a Ap 

j-i J J 


(9) 


x = x 

n 

This is the Classical Conjugate Gradient (CCG) algorithm which has been 
analyzed in considerable detail under general conditions. 

The interest in the conjugate gradient method has been further increased by 
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the development of the preconditioning strategies to accelerate convergence 
of the algorithm. Furthermore, while theCCG algorithm and its preconditioned 
versions are not naturally suitable for parallel computation, they are well 
matched for vector supercomputers, i.e., they can be efficiently vectorized 
[ 13]— [ 14] . The key concept in achieving a faster convergence resides in 
improving the condition of matrix A by preconditioning [12]. Let C be some 

nonsingular symmetric matrix and define A = C _1 AC -1 , b = C _1 b, and x = C -1 x. 
Then the algorithm can be applied to the equivalent transformed system 

A x = b where for an appropriate choice of C the convergence may be 

accelerated considerably. Let H = C 2 . The algorithm (for n steps) is written 
as [12]: 

x = 0 
o 

r = 0 
o 

For j = 1, 2, .... n 

if r =0 then set x = x and quit 
J-i j-i H 

else 


Solve MZ = r for Z 

J-i J-i j-i 


(10) 

(3 = Z t r /Z l r 
J j-l j-l J-2 j-2 

o 

III 

oa 

(ID 

p = Z + £ p 

J J-i J J-i 

TJ 

*-» 

hi 

o 

(12) 

a = Z l r /p l Ap 
J J-i J-i J J 


(13) 

x = x + a p 

J J-i J*J 


(14) 

r = r - a Ap 

J J-i J J 


(15) 


x = x 

n 

This is the Preconditioned Conjugate Gradient (PCG) algorithm and the 
symmetric positive definite matrix M is called the preconditioner. In order 
for M to be effective as a preconditioner, it is essential to be able to 
easily solve the lineair systems in Eq. (10). A well chosen preconditioner 

can lead to rapid convergence, often after 0(n ) iterations [12]. Note 

that if M 1 = A *, then the iteration converges immediately. So one hopes 

that when M 1 ~ A (in some sense) the iteration converges very quickly. In 

fact, this is what has been shown in [15]. As a result, if the matrix A is 
diagonally dominant then M = Diag (A) may be an excellent preconditioner 
since M closely approximates A. Furthermore, with the a diagonal matrix the 
solution of Eq. (10) is trivial. The choice of M = Diag (A) is known as 
Diagonal Scaling or PCG-DS. Note that, compared to the cost of each 
iteration of CCG, such a choice leads to only an additional cost of n 
divisions per iteration of PCG-DS. Given the faster convergence, this 
represents an efficient tradeoff which explains the preference for the use 
of PCG-DS over CCG even where A is not diagonally dominant. 

However, the serial and parallel computation of the conjugate gradient 
algorithms, when applied to the forward dynamics solution, differs from its 
application to more generic problems. In fact, it is usually assumed that 
the matrix A is given which is not the case for the forward dynamics problem. 
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For serial processing, note that, the basic operation in the CCG and PCG 
algorithms is the matrix-vector multiplication in Eqs. (7) and (13) with the 

computation complexity of 0( n 2 ). Given n iterations, this leads to 0(n 3 ) 
computational complexity of the algorithms. For forward dynamics problem, 
this operation represents the evaluation of the vector of joint inertia 
forces/torques, i.e. , r(J), for a given vector of joint acceleration ( Pj ), 

which can be computed in 0(n) steps, using the N-E formulation. This can 
be done for CCG algorithm without explicit computation of A which has also 
been exploited in [7]. Note that, the derivation of the dynamic models of 
the industrial manipulators, in symbolic form, shows that their inertia 
matrices can be practically considered as diagonal dominant [16]. Therefore, 
the PCG-DS algorithm can be expected to achieve a rapid convergence in 
solving the forward dynamics problem. However, the application of PCG-DS 
algorithm requires the computation of the diagonal elements of A. Hence, the 
algorithmic efficiency in computing the diagonal elements is a key factor in 
the successful application of PCG-DS algorithm to the forward dynamics 
solution. 

In the context of the forward dynamics solution, the CCG and PCG-DS also 
provide suitable features for parallel processing. Exploiting maximum 
parallelism, the matrix-vector multiplication in Eqs. (7) and (13) can be 

performed in 0(log 2 n) steps with 0(n 2 ) processors. However, besides using 

too many processors, exploitation of maximum parallelism requires a complex 
processor interconnection. For the forward dynamics problem, this operation, 
as is shown in [18], can be performed in 0[ log^n) steps with n processor and 

a rather simple interconnection. This leads to the 0(nlog 2 n) parallel CCG 

algorithm. It is shown that, using the same architecture as in [18], the 

diagonal elements of the inertia matrix can be computed in 0(log 2 n) steps 

[11]. This implies that, if PCG-DS algorithm converges in 0(n ) 

iterations, then its parallel version can achieve a computational time of 

O( n 1/Z log 2 n) with n processor and a simple processor interconnection 

structure. In fact, the parallel PCG-DS may represent the fastest stable 
algorithm for computation of the forward dynamics problem [10]. 

III. THE CCG ALGORITHM 

III.l Notations and Preliminaries 

The N-E formulation can be expressed as a function which, given Q, 

Q, Q, and F , evaluates x as [4]: 

x = g i (Q,Q,Q,F £ .) (16) 

The matrix-vector operation in Eqs. (7) and (13) is a special application of 

a which evaluates a set of vectors of inertia forces/torques as: 
i 

r(j) = g.(Q ,0,Q ,0) = g (Q ,Q ) (17) 

la j c, & j 

where Q is the vector of joint positions representing the manipulator s 

a «t 

configurations for which Eq. (17) is evaluated for a set of s. 
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z i+l 



x i 


Fig.l. Link, Frames, and Kinematic and Dynamic Parameters 

q^q^q^ position, velocity, and acceleration of joint i, respectively. 
Angular acceleration of link i 
Linear acceleration of link i (point 0^ ) . 

V Linear accelerations of center of mass of link i (point cm ). 

ic i 

F and N Force and moment exerted on center of mass of link i. 

i 1 

f and n Force and moment exerted on link i by link i-1. 

i i * 

Table I. Notion Used in the Derivation of the Algorithms. 

The major redundancy in the evaluation of Eq. (17) by the algorithm of [7] 
results from the choice of coordinate frame for projection of the intrinsic 
equations. Note that the evaluation of the original N-E formulation in link 
coordinate frames requires 0(n) transformations for link-to-link propagation 
of the variables. Hence, using the link frames for n times evaluation of Eq. 

(17), as is done in [7], requires 0(n ) transformations. However, if n times 
evaluation of Eq. (17) is performed in a fixed frame then only 0(n) 
transformations for projection of the vectors and the tensors are required. 

In deriving the algorithms, we first develop the intrinsic equations, 
i.e., the coordinate-free representation of equations. This provides a 
suitable abstraction since the equations can be derived from the intrinsic 
physical relationships, which are independent of any coordinate frame. More 
important, this allows us to distinguish between the redundancy in the 
intrinsic and that in the extrinsic equations. In order to derive the 
intrinsic equations, we need to recall some notations. In this paper, 
according to Gibbs notation, vectors are underlined once and tensors 
(tensors of order 2) twice. The projection of the vectors and the tensors 
results in 3x1 (column matrix) and 3x3 scalar matrix wherein the superscript 
denotes the coordinate frame on which the projection is performed. To any 

vector V a tensor V can be associated whose projection is a 3x3 skew 
symmetric scalar matrix as: 



0 

-V 

(z) 

v 

(y) 

V = 

V 

(z) 

0 

-V 

(x) 


-V 

L (y) 

V 

(x) 

0 


334 


I 



Note that V V = V xV = -V xV = -V V . Also, a set of notations, presented 
** 1“2 "1 "2 "2 ~ 1 = 2~1 

in Fig. 1 and Table I, are used in the derivation of the algorithms. 

III. 2 A Variant of The N-E Formulation 

Let us write the N-E formulation for link i (Fig. 1) with the nonlinear 
terms being excluded. 

(18) 

(19) 

( 20 ) 

( 21 ) 

( 22 ) 

(23) 

(24) 

(25) 


♦ 

0) 

i 

ss 

d> + z q 

1-1 “li 

V 
~ 1 

= 

V + d) x 

-J-1 “l-l 

V 

“ic 

rs 

V + t) x S 
“i “l ~i 

F 

“l 

= 

m V 

l“lc 

N 

= 

J U) 
*1-1 

f 

“i 

= 

F + f 
“i “l+i 

n 

“i 

= 

N + S x F 
“i “i ”i 

r 

i 

- 

z . n 
“i “i 


"l-i 


i + P x f 

"i+i - i - i+i 


l 

of joint i. Eqs. (18) -(25) describe the procedure for computation of the 
vector T(j) or the function g . Note that, for the sake of simplicity, an 

all revolute joints manipulator is considered. However, with small changes, 
the results can be extended to the manipulator with sliding joint(s). 


A variant of g can be derived by replacing Eqs. (20)-(22) into Eqs. 
2 


(26) 


(23) and (24) as 

f, = m i V + 6 t x («£> + £ 1+1 

Q, = ij" + x (m i V i + ux (m^)] + n +1 + P x £ 1+1 

- + (■,§,)* k + q 1+1 + e, x ^ (27) 

The terms J -n^S^ and m^ represent the first and the second moment of 

mass of link i with respect to point 0 j which are designated as and h^ 

respectively. Note that k and are constant in link i coordinate frame, 

i.e. , coordinate frame 1+1, and can be given as the link parameters. The 

variant of the N-E formulation for computation of the vector T, designated 

as a , is written as: 

3 

(28) 

(29) 

(30) 

(31) 

(32) 


it> 

“i 

= 

0> 

i -1 

+ 

z q 

V 

— i 

= 

V 

“i-1 

+ 

d> x P 

-l-i -] 

f 

“1 

= 

m V 

l-i 

+ 

d) x h 
“i -l 

n 

“i 

= 

k £) 

=i—i 

+ 

h x V 

“i “i 

r 

i 

= 

z . n 
-l “ 

i 



-l+i 

+ n + P x f 

-i+i “i “i+i 


All tuc cxvjyj v c pi ^ 1 

of, and the force and the moment exerted on, the link's center of mass is 
avoided. Note that the computation cost of g g and g 3 is the same. However, 
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if the equations of both procedures are projected on some fixed coordinate 

frame then evaluation of g requires the transformation of J and S while 

2 11 

that of g 3 requires the transformation of and h . For the CCG algorithm, 
since the evaluation of the original N-E formulation, i.e., g , requires the 
transformation of J and S , it is more efficient to use g . However, g 

11 2 3 

will be used to derive the algorithm for computation of the diagonal 
elements of the inertia matrix and the evaluation of f in PCG-DS algorithm. 


III. 3 Computation of the CCG Algorithm 

As stated before, it is more efficient to project the equations on some 
fixed frame. To do so, P , S , z ^ , and J should be projected onto the fixed 

frame. We use the EE (n+1 th) coordinate frame which is slightly more 
efficient since P , S , z , and J are constant in this coordinate frame. 

n “h “n 

Let m and a denote the cost of multiplication and addition, respectively. 

The computational steps of the CCG algorithm are performed as follows where, 
for each step, its computational cost is also indicated. 

Step 1: Projection of the vectors and the tensors 


For i = 1 , 2, . . . , 
1) Evaluate i+1 R 


n 


2) 

3) 

4) 

5) 

6 ) 


n+l n 


n+l_, 1 

R 

ss 

R 

l 


l+l 

n+l„ 


n+l„ 

z 

= 

R Z 

i 


i 0 

n+l_ 


n+1^ i 

S 


R S 

i 


i 

n+l_ 


n+ 1 _ i_ 

p 


R P 

i 


i ! 

n+l - 


n+l_. 1 . 

j 

= 

R J 


l+lr 


With z = [0 
o 


1] 


R 

i n+l 


(33) 

(34) 

(35) 

(36) 

(37) 


The computation cost of this step is obtained as 4nm + (n-l)(96/n + 63a). In 
the following the absence of the superscript denotes that the vectors and 
the tensors are described with respect to the EE coordinate frame. 

Step 2: Computation of r Q = t - r 


1) Compute x = g (Q ,Q ,Q ,F ) 

a 1 a a a e 

a) For i = 1 , 2, . . . , n 


0) = 

U + 

z q 

1 

i-1 

1 ai 

0) = 

U) + 

U) X 

1 

i-1 

i-1 

V = 

V + 

0) X 

i 

i-i 

i-1 

* 

♦ 


V = 

V + 

o> x S 

ic 

i 

i i 

t! II 

Uh 25 

m V 

1 ic 

J d> + 

0 ) x(J 

i 

i i 

i 


: q + Z q 

I ai i ai 

5 + o> x(w X P ) 

i -1 i-1 i-1 i -1 


D = 0 
1 

(I) = 0 
1 

V = GZ 

l l 


i i 


b) For i = n, n-1, 
f = F + f 

i i i +i 


f = f 

n+l E 


(38) 

(39) 

(40) 

(41) 

(42) 

(43) 

(44) 
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Algorithm 


General 


n = 6 


Mul . Add. Mul . Add. 


CCG in [7] 

76n 2 +120n-21 

56n 2 +87n-6 

3435 

2532 

This paper 

47n 2 +177n-117 

46n 2 +118n-87 

2637 

2277 


Table II. Comparison of the CCG algorithms 


n = N + S x F 
1 l i 

x = n . z 


al i 1 

2) Compute r Q = t 


i 


+ n 


i ♦! 


T 

a 


+ P X f 
1 


1+1 


n = n (45) 

n+l E 

(46) 

(47) 


Note that G = 9.8061m/s 2 denotes the acceleration due to the gravity which 
is along the direction of z . The cost of this step is n(87m+78a)-(21m+24a) . 

The rest of the computation is carried out according to Eqs. (5) -(9) where 
the matrix-vector operation in Eq. (7) is performed by using the function 
g (Q ,p ). Each iteration of Eqs. (5)-(9) requires n(47m+46a)-( 10m+23a) 

which, taking n iterations, leads to the total cost of the CCG algorithm as 

n 2 (47m+46a)+n(177m+118a)-(117m+87a). The cost of the developed algorithm is 
compared to the CCG algorithm of [7], Note that the algorithm of this paper 
achieves a better efficiency by a significant reduction in the coefficient 
2 

of n terms. 


IV. THE PCG-DS ALGORITHM 

IV. 1 An Algorithm for Computation of the Diagonal Elements of Inertia Matrix 


From Eq. (3) the diagonal elements of the inertia matrix can be 
computed els 


a = r 

li l 


(48) 


for the conditions given by 

q = 1 and q =0 For k = 1, 2, .... n 

M i ic*i 

An algorithm for computation of the terms a , using g , can be derived as 

i i J 


(49) 


a = g (Q ,e ) 

11 °31 al 1 


(50) 


where subscript i denotes that g is evaluated for the last n-i + 1 links, 
is the vector of actual position of the last n-i+1 joints, e^ is an ixl 

vector as e ( = [1 0 ... O] 1 . With the conditions given by Eq. (49), let 

u and V , and f and n ( j> i ) stand for angular and linear 
j(i) j ( l ) J < l ) Jd) 

acceleration of, and force and moment exerted on, link j (point 0^) due to 
the unit acceleration of joint i. For link J, Eqs. (28)-(31) are written as 


337 


( 51 ) 


u> = z 

-J(i) “1 

V = z xP 

~jd) “1 “M 

f =m(zxP )+zxh + f 
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-J<1) =J“i “j “i — J . 1 J +1(1) ”j -j+l(i) 

and, for link i, these equation are written as 
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= 
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= 
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Using Eqs. (51) -(54), Eqs. (57) -(58) car be rewritten as 

f =zxh+V fm(zxP )+zxhl=zx[h+r [m P + h 1 
i < i ) “i “i L k “ 1 k , 1 “I “k “i “i L k“k,i “k 

k=i+l L J L k=i+l L J 
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n 
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h + Y 
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n =kz + V k z + h x(z xP ) + P x m z xP + z xh 
-i(i) =l“i L =k“l “k — i -k,i “k,i k “i “k,i — i “k 

k=i+l L J 

= {k + Y [k -m P P - h P - P h llz = K z (61) 

[=1 k=k,i=k,i =k=k,i =k,i=kJJ“i =l - i 

Note that the conditions given by Eq. (49) imply that link i through link 
n do not have ary relative motion, i.e., are rigidly connected, and form a 
composite rigid-body. In comparison with Eqs. (57)-(58), and ^represent 

the first ard the second moment of mass of the composite system composed of 
link i through link n (denoted as composite system i) about point 0^ . From 

Eqs. (60)-(61), the recursions for computation of and are derived as 


f i- n 

1 

lh + y 

m P + h 

l-i+l L 

t k=i +2 

k“k,i+2 “kj 


where is the mass (zeroth moment mass) of the composite system i+1, and 


^ ’ A A A A A A 

K = k + V k-mP P -hP -P h = k 

=1 =i L =k k=k,i=k,i =k=k,l =k,l=k =1 

lr=l *1 L J 


Y Ik -m (P +P )(P +P ) -h (P +P ) - (P +P )h ] 

L =k k =k,i+l =1 =k,i+l =1 “k =k, 1+1 “1 =k,i+l =1 =kl 

k=l+l L J 

which after some manipulations and by using Eq. (62) can be written as 

K=k+M PP-H P-PH + -fk +V [k-mP P . - 
=1 =1 1+ 1=1=1 =1+1=1 =1=1+1 1=1 + 1 L =k k=k,l+l=k,l+l 
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„ ~ ~ ~ -1 ~ ~ ~ ~ 

hP -P h =k-M PP-H P-PH + K 

§k=k,i+i =k,i+i=kj =1 i+i=i=i =i+i=i =1=1+1 “1 +i 


(63) 


The diagonal elements of the inertia matrix, or M = Diag(A), are computed as 


For i = n, n-1 , .... 1 
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+ m 
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(64) 

(65) 

( 66 ) 

(67) 


It should be pointed out that Renaud 1 17 1 used the notion of augmented- 
body to derive the equations similar to Eqs. (62)-(63). However, our 
derivation of Eqs. (62)-(63) shows that this notion is implicit in the N-E 
formulation. The improved efficiency of the above algorithm, compared to the 
composite rigid-body algorithm in [7], results from the elimination of the 
redundancy in the intrinsic equations. Note that by directly computing the 
first and second moment of mass of composite system i about point the 


redundant computation of the center of mass of composite systems and the 
force and moments acting on the centers of mass are avoided (see [8] for 
more discussion regarding these two algorithms). 


IV. 2 Computation of the PCG-DS Algorithm 

Eqs. (64)-(67) describe the intrinsic relation between the diagonal 
elements of the inertia matrix, which are scalar, and the links kinematic 
and dynamics parameters, which consist of scalars, vectors, and tensors. In 
order to compute the diagonal elements, Eqs. (65)-(67) should be projected 
on some coordinate frame. Due to the evaluation of g^ and g^(or g^) in EE 

frame, it is more efficient to project Eqs. (65)-(67) on this frame which 
allows the exploitation of synergism in different computations. To do so 
h and k need to be evaluated in EE frame and then H , K , and a can be 

ii i i ii 

computed from Eqs. (65M67) with the vectors and tensors being described 
with respect to EE frame. The cost of evaluating M is then obtained as 
(n-1 ) (51m+50a) where the symmetry of matrices in Eq. (66) is exploited. Note 
that the additional cost of PCG-DS algorithm, due to the evaluation of M, is 
almost equal to one iteration of Eqs. (5)-(9) for CCG algorithm. The best 
algorithm for computation of inertia matrix requires n(69m+62a)-(57m+58a) 
for evaluation of the diagonal elements [8]. Hence, the geater efficiency of 
the developed algorithm results from the exploitation of synergism in 
different computation. Having computed M, the second step of the PCG-DS 
algorithm is performed similar to CCG algorithm and the rest of the 
computation is carried out according to Eqs. ( 10 )— ( 15 ) . 


V. CONCLUSI ON 

In this paper we investigated the applicability of conjugate gradient 
algorithms for computation of the manipulator forward dynamics. Two 
algorithms were presented and their computational efficiency was analyzed. 
The preconditioned algorithm is particularly promising because of its 
potentially rapid convergence as well as its suitability for parallel 
computation. We are currently investigating in greater detail the effect of 
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the preconditioner on the convergence of the algorithm. This work includes 
analysis of error estimates as well as simulations with actual manipulators. 
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